//--- Projeto Domus
// interactionsource.com
// Koji Pereira, Andre Veloso e Marcos Paulo

/*
 Projeto Domus
 interactionsource.com
 Koji Pereira, Andre Veloso e Marcos Paulo

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 3 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/


import processing.opengl.*;
import processing.serial.*;
import oscP5.*;
import netP5.*;
import cc.arduino.*;

Arduino arduino;
int sensorPin0 = 0;
int sensorPin1 = 1;
int sensorPin2 = 2;
int sensorPin3 = 3;

int sensor1 = 0;
int sensor2 = 0;
int sensor3 = 0;
int sensor4 = 0;

boolean isSensor1Send = false;
boolean isSensor2Send = false;
boolean isSensor3Send = false;
boolean isSensor4Send = false;

int CONST_THRESHOLD_LUZ = 600;

OscP5 oscP5;

float xrot = 0;
float zrot = 0;

float xrot_targ = 0;
float zrot_targ = 0;
float orientation = 0;

float valX;
float valY;
float valZ;

float dampSpeed = 5;

float resultante;

float oldResultante;

float diffResultante;

float varResultante = 0;

float oldValX = 0;
float oldValY = 0;
float oldValZ = 0;

float accelX = 0;
float accelY = 0;
float accelZ = 0;

float accelVector = 0;

float diffX = 0;
float diffY = 0;
float diffZ = 0;

float pitch, roll = 0;

PFont fontA;

NetAddress myRemoteLocation;

OscMessage myMessage;

int CONST_GRAVITY_THRESHOLD = 28;

int contZTempoParado = 0;
int contZTempoMovendo = 0;

int CONST_PARADO = 1;
int CONST_MOVENDO = 2;

int estadoAtual = 1; //inicia com estado atual PARADO

int CONST_DESVIO_VALOR_POSICAO = 4;

//coordenadas do vetor de forca das faces do dado de 12 ladas, envidadas pelo wiimote
float[][] dado = {  
  {   0,   0, -24},
  {   0,   0,  24},
  {   4,  18, -10},
  {  -9,  16,  10},
  {  15,  12,  10},
  { -20,   8, -10},
  {  20,   0, -10},
  {  -4, -18,  10},
  { -15, -12, -10},
  {   9, -16, -10},
  {  20,  -8,  10},
  { -20,   0,  10}
};

int posDado = 0;      //indica a face do dado que estah apontada para cima
int oldPosDado = 20;  
boolean isEnviaMsg = false;

void setup() {
     size(640,480, P3D);
     // smooth();
     
     fontA = loadFont("SansSerif-30.vlw");
     textFont(fontA, 20);
     
    /* myRemoteLocation is a NetAddress. a NetAddress takes 2 parameters,
     * an ip address and a port number. myRemoteLocation is used as parameter in
     * oscP5.send() when sending osc packets to another computer, device, 
     * application. usage see below. for testing purposes the listening port
     * and the port of the remote location address are the same, hence you will
     * send messages back to this sketch.
    */
    //Inicia envio OSC
    oscP5 = new OscP5(this, 6001);
    myRemoteLocation = new NetAddress("192.168.2.2", 5000);
    
    //imprime o array do dado
    println("--- COORDENADAS DO DADO ---");
    for (int i=0; i<12; i++) {
     for (int j=0; j<3; j++) {
       print (dado[i][j] + ", ");
     }
       println("");
    } 

}

void draw() {

  
 background(255);

    
    //----------------------------------------------------------------------------

    
    oldResultante = resultante;
    diffResultante = resultante - 35;

    oldValX = valX;
    oldValY = valY;
    oldValZ = valZ;
    
    
    //envia mensagem OSC para a posicao do dado, somente quando corre modificacao e somente 1 mensagem
    if ( isEnviaMsg ) {
      enviaMensagemOSC_Int(posDado, "/dado/pos" , myRemoteLocation);
      println("Enivado posDado em OSC : " + posDado);
      isEnviaMsg = false;
    } 
    

    //---------------------------------------------------------------------------------
    // INICIO SAIDA GRAFICA 
    
    fill(0);
    text("Forca Res:", 30, 30);
    text(resultante/34, 150, 30);
    text("G:", 220, 30);

    text("Posicao dado", 30, 60);
    text(posDado , 170, 60);

    
      stroke(0);
      strokeWeight(50);
      line(30, 300, 30, 301-varResultante );
    
    
      pushMatrix();
      translate(width/2, height/2, 1);
      
    
      rotateX(-PI/6*(mouseX*0.01));
      rotateY(-PI/6*(mouseY*0.01));
      
      
      fill(0);
      text("+Z:", 0, -100);
      text("-Z:", 0, 100);
      
      fill(0);
      text("-Y:", -100, 0);
      text("+Y:", 100, 0);
      
      fill(0);
      text("-X:", 0, 0, -100);
      text("+X:", 0, 0, 100);
      
      //Eixos
     
      stroke(255,255,0);
      strokeWeight(2);
      line(-600,0,0, 600, 0,0);
      
      stroke(0,255,255);
      strokeWeight(2);
      line(0,-600,0, 0, 600,0);
      
        stroke(255,0,255);
      strokeWeight(2);
      line(0,0,-600, 0, 0, 600);
      
    
      //print("diffResultante: " + diffResultante);
    
      fill(127);
      noFill();
    
    
      strokeWeight(2);
      box(70,10,40);
    
    
      //Vetor resultante
      stroke(0);
      strokeWeight(10);
      line(0,0,0, valY*1.5, valZ*1.5, valX*1.5);
    
      //-------------------------------------
      
      //Vetor forca individual
      stroke(255,255,0);
      strokeWeight(5);
      line(0,0,0, -valY*1.5, 0,0);
    
    
      //Vetor forca individual
      stroke(0,255,255);
      strokeWeight(5);
      line(0,0,0, 0, -valZ*1.5,0);
    
    
    
      //Vetor forca individual
      stroke(255,0,255);
      strokeWeight(5);
      line(0,0,0, 0, 0, -valX*1.5);
      
    
       //------------------------------
      /*
      //Vetor forca individual
      stroke(255,255,0);
      strokeWeight(5);
      line(0,0,0, -accelY*1.5, 0,0);
    
    
      //Vetor forca individual
      stroke(0,255,255);
      strokeWeight(5);
      line(0,0,0, 0, -accelZ*1.5,0);
    
    
      //Vetor forca individual
      stroke(255,0,255);
      strokeWeight(5);
      line(0,0,0, 0, 0, -accelX*1.5);
      */
    
      //----------------------------------------
      //Vetor accel resultante
      stroke(255,0,0);
      strokeWeight(10);
      line(0,0,0, accelY*1.5, accelZ*1.5, accelX*1.5);
    
 
      popMatrix();
    
      // FIM SAIDA GRAFICA 
      //---------------------------------------------------------------------------------
    

}


/*
 * Envia uma mensagem OSC contendo um int para o dest especificado
 */
void enviaMensagemOSC_Int(int intVal, int lowRange, int highRange, String oscMessage, NetAddress dest) {
   
       //Converte para a gama aceitavel em OSC, de 0 a 1.0
      float valSend =  map(intVal, lowRange, highRange, 0.0, 1.0);
      valSend = constrain(valSend, 0.0, 1.0);
   
      myMessage = new OscMessage(oscMessage);
      myMessage.add(round(valSend)); // add an int to the osc message 
      oscP5.send(myMessage, dest); 
}

/*
 * Envia uma mensagem OSC contendo um int para o dest especificado
 */
void enviaMensagemOSC_Int(int intVal, String oscMessage, NetAddress dest) {
   
      myMessage = new OscMessage(oscMessage);
      myMessage.add(intVal); // add an int to the osc message 
      oscP5.send(myMessage, dest); 
}

/*
 * Envia uma mensagem OSC contendo um float para o dest especificado
 */
void enviaMensagemOSC_Float(float floatVal, int lowRange, int highRange, String oscMessage, NetAddress dest) {
   
      //Converte para a gama aceitavel em OSC, de 0 a 1.0
      float valSend =  map(floatVal, lowRange, highRange, 0.0, 1.0);
      valSend = constrain(valSend, 0.0, 1.0);
    
      myMessage = new OscMessage(oscMessage);
      myMessage.add(floatVal); // add an int to the osc message 
      oscP5.send(myMessage, dest); 
  
}


/*
 * Recepcao de mensagens OSC
 */
void oscEvent(OscMessage theOscMessage) {


    String addr = theOscMessage.addrPattern();
    //println(addr);   // uncomment for seeing the raw message
    //float  val  = theOscMessage.get(0).floatValue();




   if(addr.indexOf("/wiimotePD/gX") !=-1){ // one of the faders
       valX  = Float.valueOf( theOscMessage.get(0).intValue() );
       //valX /= 100;


    }

    if(addr.indexOf("/wiimotePD/gY") !=-1){ // one of the faders
       valY = Float.valueOf( theOscMessage.get(0).intValue() );
       //valY /= 100;

    }

   if(addr.indexOf("/wiimotePD/gZ") !=-1){ // one of the faders
       valZ  = Float.valueOf( theOscMessage.get(0).intValue() );
        //valZ  /= 100;
    }
   
 
    //DEBUG POSICAO DADO
    /*
    for (int i=0; i<12; i++) { 
      print("valor X: " + valX + " val+: " + (dado[i][0] + CONST_DESVIO_VALOR_POSICAO) + " val-: " + (dado[i][0] - CONST_DESVIO_VALOR_POSICAO) );
      print(" - valor Y: " + valY + " val+: " + (dado[i][1] + CONST_DESVIO_VALOR_POSICAO) + " val-: " + (dado[i][1] - CONST_DESVIO_VALOR_POSICAO) );
      println (" - valor Z: " + valZ + " val+: " + (dado[i][2] + CONST_DESVIO_VALOR_POSICAO) + " val-: " + (dado[i][2] - CONST_DESVIO_VALOR_POSICAO) );
    }
    */
   
    //pesquisa na tabela de coordenadas a possivel posicao da face superior do dado

    for (int i=0; i<12; i++) {
       
       if ( 
            ( ( valX < dado[i][0] + CONST_DESVIO_VALOR_POSICAO ) && ( valX > dado[i][0] - CONST_DESVIO_VALOR_POSICAO ) ) &&
            ( ( valY < dado[i][1] + CONST_DESVIO_VALOR_POSICAO ) && ( valY > dado[i][1] - CONST_DESVIO_VALOR_POSICAO ) ) &&
            ( ( valZ < dado[i][2] + CONST_DESVIO_VALOR_POSICAO ) && ( valZ > dado[i][2] - CONST_DESVIO_VALOR_POSICAO ) )
            
            ) {
              oldPosDado = posDado;
              posDado = i+1;
              
            } 

       
    } 
    
    if ( oldPosDado != posDado ) {
      isEnviaMsg = true;
    }

   //println("posDado: " + posDado );


       //calculo dos valored lidos do wiimote
       resultante = sqrt( sq(valX) + sq(valY) + sq(valZ) );

       varResultante = (resultante-oldResultante) ;

       if (varResultante<0)
         varResultante *= -1;

       accelVector = sqrt( sq(accelX) + sq(accelY) + sq(accelZ) );


         accelX = valX - oldValX;
         accelY = valY - oldValY;
         accelZ = valZ - oldValZ;


}

//Tratamento de entrada do teclado
void keyPressed() {
  
  if (key == 'z') {
    sensor1 = 700;
  }
  
  if (key == 'x') {
    sensor2 = 700;
  }
  
  if (key == 'c') {
    sensor3 = 700;
  }

  if (key == 'v') {
    sensor4 = 700;
  }
  
}

void keyReleased() {
  
  if (key == 'z') {
    sensor1 = 100;
  }
  
  if (key == 'x') {
    sensor2 = 100;
  }
  
  if (key == 'c') {
    sensor3 = 100;
  }

  if (key == 'v') {
    sensor4 = 100;
  }  
  
}

